#!/usr/bin/env python

##
##
 
import rospy
from std_msgs.msg import Header

#from object_recognition_msgs.msg import RecognizedObjectArray



from group6.msg import DetectedObject
from group6.msg import DetectedObjectArray

# for dummy
from fixedToyLocations import createObjectList

#def objCallback(data):
#    if ( len(data.objects) > 0 ):
#        rospy.loginfo("###")
#        for o in data.objects:
#            pos = o.pose.pose.pose.position
#            rospy.loginfo("x:%.2f y:%.2f z:%.2f", pos.x, pos.y, pos.z)
        
# main code
def object_detection():

    #Publisher and stuff
    pub = rospy.Publisher('detected_object_array', DetectedObjectArray, queue_size=10)
    
    #Init the node    
    rospy.init_node('object_detection', anonymous=True)

    #Subscribe to /recognized_object_array
    #rospy.Subscriber("recognized_object_array", RecognizedObjectArray, objCallback)
	
    r = rospy.Rate(10) #10Hz, added    

    #seq = 0
    while not rospy.is_shutdown():
        # increment seq
        #seq = seq + 1
                
        # create a new DetectedObjectArray
	arr = DetectedObjectArray()
        
        # create header
        arr.header = rospy.Header()
        #header.frame_id = frame
        arr.header.stamp = rospy.get_rostime()
                
        # create a list of toys that have a fixed pre-known location, see fixedToyLocations.py for modifications
        arr.objects = createObjectList()		
        # the size of the array
        arr.size = len(arr.objects)
        
        # log
        rospy.loginfo(arr)
        # publish
        pub.publish(arr)
        r.sleep()
    
    # spin() simply keeps python from exiting until this node is stopped
    #rospy.spin()
        
if __name__ == '__main__':
    try:
        object_detection()
    except rospy.ROSInterruptException: pass

